
#include "mem_struct.h"
#include "types.h"

#define round8(var) do{ if(*var & 7) *var = (*var & ~7) + 8; }while(0)

/**
 * Fast Speicherknoten mit nachfolger zusammen, wenn diese hintereinander 
 * liegen (zum defragmentieren bei Freigabe eines Speicherblockes)
 */
static void defrag(mem_node_t *node);

mem_struct_t *
mem_init( mem_struct_t *ms, void* start, unsigned long size)
{
    ms->list = (mem_node_t*)start;
    ms->list->next = NULL;
    ms->list->size = size;

    return ms;
}

void *
mem_alloc( mem_struct_t *ms, unsigned long size )
{
    if(size <= 0) return NULL;

    mem_node_t head = { 0, ms->list };
    mem_node_t *cur = &head;
    mem_node_t *node = NULL; //return value

    //round to power of 8
    round8(&size);

    //advance to next node
    for(; cur->next && cur->next->size < size; cur = cur->next );
    node = cur->next;
    if(node){ //yeah, we have found a node
        if(node->size == size) cur->next = node->next;
        else { //split node
            mem_node_t *new_node = (mem_node_t*)(((unsigned long)node) + size);
            new_node->size = node->size - size;
            new_node->next = node->next;
            cur->next = new_node;
        }
    }
    ms->list = head.next;

    return node;
}

void 
mem_free( mem_struct_t *ms, void* ptr, unsigned long size)
{
    if(!size || !ptr) return;

    mem_node_t head = {0, ms->list };
    mem_node_t *cur = &head;

    round8(&size);

    //advance to correct position
    for(; cur->next && (unsigned long)cur->next <= (unsigned long)ptr; cur = cur->next);

    mem_node_t *new_node = (mem_node_t*)ptr;
    new_node->size = size;
    new_node->next = cur->next;
    cur->next = new_node;

    defrag(new_node);
    defrag(cur);

    ms->list = head.next;
}

static void 
defrag(mem_node_t *node)
{
    if(node && node->next &&
       (((unsigned long)node) + node->size == ((unsigned long)(node->next))))
    {
        node->size += node->next->size;
        node->next = node->next->next;
    }
}

